#include "SF_BLDC.h"
#include "SF_Serial.h"
#include "pid.h"

SF_BLDC_DATA  BLDCData;
SF_BLDC motors = SF_BLDC(Serial2);  //定义与S1通信的对象
SF_Serial_DATA receive_openmv;   //定义openmv接收数据对象


void setup() {
  Serial.begin(921600);
  Serial1.begin(921600, SERIAL_8N1, 36, 37);    //openmv
  motors.init();
  motors.setModes(2,2);
}

float M1_angle;
float M0_angle;
void loop() {
  BLDCData = motors.getBLDCData();                     
  M1_angle = BLDCData.M1_Angle;  //接收Wroom电机运行速度到 driver 对象
  M0_angle = BLDCData.M0_Angle; 
  receive_openmv = Serial_Data();

  if( Serial_Contact() == 1){ 
    motors.setTargets(M0_angle,M1_angle - receive_openmv.M1_Target); 

  }
}
